within ThreeD_MBS_Dynamics.Examples.OmniVehicle.PointContact;

model AutonomousPatchContactOmniWheelSetTest2
  parameter Integer n = 4 "Number of rollers";
  parameter Real alpha = Modelica.Constants.pi/n
    "Max angle of the half-sector";
  parameter Real R = 1 "Omni wheel outer radius";
  parameter Real d = 2 "vehicle size";
  parameter Real R1 = R*cos(alpha) "Omni wheel inner radius";
  parameter Real om0 = 0.1;
  parameter Real[3] r0 = {0, R, 0};
  parameter Real[4] q0 = {1, 0, 0, 0};
  parameter Real[3] v0 = {0, 0, 0};
  parameter Real[3] omega0 = {0, om0, 0};
  parameter Real pi = Modelica.Constants.pi;
//  Real[3] d;
  Base Floor 
            annotation (Placement(transformation(extent={{-90,-10},{-70,10}})));
  annotation (Diagram(coordinateSystem(preserveAspectRatio=true, extent={{-100,
            -100},{100,100}}),
                         graphics),
    experiment(
      StopTime=3,
      NumberOfIntervals=30000,
      Tolerance=1e-009,
      Algorithm="Dassl"),
    experimentSetupOutput);
  FixedJoint Joint1(
    nA = {0, 0, 1},
    nB = {1, 0, 0},
    rA = {0, 0, 0},
    rB = {d, 0, 0}) 
    annotation (Placement(transformation(extent={{20,40},{40,60}})));
  FixedJoint Joint2(
    nA = {0, 0, 1},
    nB = {-cos(pi/3), 0, -cos(pi/6)},
    rA = {0, 0, 0},
    rB = {-d*cos(pi/3), 0, -d*cos(pi/6)}) 
    annotation (Placement(transformation(extent={{20,-10},{40,10}})));
  ThreePortsHeavyBody Platform(
    Gravity = {0, -1, 0},
    r(start = r0),
    v(start = v0),
    q(start = q0),
    omega(start = omega0)) 
    annotation (Placement(transformation(extent={{70,-10},{90,10}})));
  OmniWheel1 Wheel1(
    Gravity={0,-1,0},
    T0=[0,0,-1; 0,1,0; 1,0,0],
    r0=r0 + {d,0,0},
    v0=v0 + cross(omega0, {d,0,0}),
    q0=QMult(q0, {cos(pi/4),0,sin(pi/4),0}),
    omega0={0,om0,-d*om0/R}) 
    annotation (Placement(transformation(extent={{-40,40},{-20,60}})));
  OmniWheel1 Wheel2(
    Gravity={0,-1,0},
    T0=[-cos(pi/6),0,cos(pi/3); 0,1,0; -cos(pi/3),0,-cos(pi/6)],
    r0=r0 + {-d*cos(pi/3),0,-d*cos(pi/6)},
    v0=v0 + cross(omega0, {-d*cos(pi/3),0,-d*cos(pi/6)}),
    q0=QMult(q0, {cos(7*pi/12),0,sin(7*pi/12),0}),
    omega0={0,om0,-d*om0/R}) 
    annotation (Placement(transformation(extent={{-40,-10},{-20,10}})));
  OmniWheel1 Wheel3(
    Gravity={0,-1,0},
    T0=[cos(pi/6),0,cos(pi/3); 0,1,0; -cos(pi/3),0,cos(pi/6)],
    r0=r0 + {-d*cos(pi/3),0,d*cos(pi/6)},
    v0=v0 + cross(omega0, {-d*cos(pi/3),0,d*cos(pi/6)}),
    q0=QMult(q0, {cos(-pi/12),0,sin(-pi/12),0}),
    omega0={0,om0,-d*om0/R}) 
    annotation (Placement(transformation(extent={{-40,-60},{-20,-40}})));
  FixedJoint Joint3(
    nA = {0, 0, 1},
    nB = {-cos(pi/3), 0, cos(pi/6)},
    rA = {0, 0, 0},
    rB = {-d*cos(pi/3), 0, d*cos(pi/6)}) 
    annotation (Placement(transformation(extent={{20,-60},{40,-40}})));
equation
//  d = cross({0, 1, 0}, Wheel.OutPort.T*{0, 0, 1});
//  Wheel.OutPort.epsilon*d/sqrt(d*d) = 0;
  connect(Floor.OutPort, Wheel2.InPortK) annotation (Line(
      points={{-80,-8},{-80,-20},{-34,-20},{-34,-9}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel1.OutPortK,Joint1. InPortA) annotation (Line(
      points={{-26,41},{-26,30},{26,30},{26,42}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel2.OutPortK, Joint2.InPortA) annotation (Line(
      points={{-26,-9},{-26,-20},{26,-20},{26,-8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel1.InPortF,Joint1. OutPortA) annotation (Line(
      points={{-26,59},{-26,70},{26,70},{26,58}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel2.InPortF, Joint2.OutPortA) annotation (Line(
      points={{-26,9},{-26,20},{26,20},{26,8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Platform.OutPort,Joint1. InPortB) annotation (Line(
      points={{80,-8},{80,-20},{60,-20},{60,30},{34,30},{34,42}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Platform.OutPort, Joint2.InPortB) annotation (Line(
      points={{80,-8},{80,-20},{34,-20},{34,-8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Joint2.OutPortB, Platform.InPort1) annotation (Line(
      points={{34,8},{34,20},{80,20},{80,8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Joint1.OutPortB, Platform.InPort) annotation (Line(
      points={{34,58},{34,70},{74,70},{74,8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Floor.OutPort, Wheel3.InPortK) annotation (Line(
      points={{-80,-8},{-80,-70},{-34,-70},{-34,-59}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel1.InPortK, Floor.OutPort) annotation (Line(
      points={{-34,41},{-34,30},{-50,30},{-50,-20},{-80,-20},{-80,-8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel3.OutPortK, Joint3.InPortA) annotation (Line(
      points={{-26,-59},{-26,-70},{26,-70},{26,-58}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Joint3.InPortB, Platform.OutPort) annotation (Line(
      points={{34,-58},{34,-70},{80,-70},{80,-8}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Wheel3.InPortF, Joint3.OutPortA) annotation (Line(
      points={{-26,-41},{-26,-30},{26,-30},{26,-42}},
      color={0,0,255},
      smooth=Smooth.None));
  connect(Joint3.OutPortB, Platform.InPort2) annotation (Line(
      points={{34,-42},{34,-30},{66,-30},{66,30},{86,30},{86,8}},
      color={0,0,255},
      smooth=Smooth.None));
end AutonomousPatchContactOmniWheelSetTest2;
